


#include "net.h"
#include "eth.h"
#include "arp.h"
#include "ping.h"
#include "autoconfig.h"


qe_u8 net_bcast_addr[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
static struct net_gd ngd;



struct net_gd *net_get_gd(void)
{
	return &ngd;
}

unsigned int net_cksum(qe_u8 *ptr, int len)
{
	unsigned long xsum;
	unsigned short *p = (unsigned short *)ptr;

	xsum = 0;
	while (len-- > 0)
		xsum += *p++;
	xsum = (xsum & 0xffff) + (xsum >> 16);
	xsum = (xsum & 0xffff) + (xsum >> 16);
	return xsum & 0xffff;
}

int net_cksum_ok(qe_u8 *ptr, int len)
{
	return !((net_cksum(ptr, len) + 1) & 0xfffe);
}

int net_set_ether(qe_u8 *pkt, qe_u8 addr[6], qe_u16 prot)
{
    struct ethernet_hdr *hdr = (struct ethernet_hdr *)pkt;

    qe_memcpy(hdr->dst, addr, 6);
    qe_memcpy(hdr->src, ngd.hwaddr, 6);
    hdr->protlen = qe_htons(prot);
    return ETHER_HDR_SIZE;
}

void net_set_ip_header(qe_u8 *pkt, ipaddr_t dest, ipaddr_t source)
{
	struct ip_udp_hdr *ip = (struct ip_udp_hdr *)pkt;

	/*
	 *	Construct an IP header.
	 */
	/* IP_HDR_SIZE / 4 (not including UDP) */
	ip->ip_hl_v  = 0x45;
	ip->ip_tos   = 0;
	ip->ip_len   = qe_htons(IP_HDR_SIZE);
	ip->ip_id    = qe_htons(ngd.ip_pkt_id++);
	ip->ip_off   = qe_htons(IP_FLAGS_DFRAG);	/* Don't fragment */
	ip->ip_ttl   = 255;
	ip->ip_sum   = 0;
	/* already in network byte order */
    qe_memcpy(&ip->ip_src, &source, sizeof(ipaddr_t));
	/* already in network byte order */
    qe_memcpy(&ip->ip_dst, &dest, sizeof(ipaddr_t));
}

void net_set_udp_header(qe_u8 *pkt, ipaddr_t dest, int dport, int sport, int len)
{
    struct ip_udp_hdr *ip = (struct ip_udp_hdr *)pkt;

	/*
	 *	If the data is an odd number of bytes, zero the
	 *	byte after the last byte so that the checksum
	 *	will work.
	 */
    if (len & 1)
        pkt[IP_UDP_HDR_SIZE + len] = 0;
    
    net_set_ip_header(pkt, dest, ngd.ipaddr);
    ip->ip_len   = qe_htons(IP_UDP_HDR_SIZE + len);
    ip->ip_p     = IPPROTO_UDP;
    ip->ip_sum   = ~net_cksum(pkt, IP_HDR_SIZE >> 1);

    ip->udp_src  = qe_htons(sport);
    ip->udp_dst  = qe_htons(dport);
    ip->udp_len  = qe_htons(UDP_HDR_SIZE + len);
    ip->udp_xsum = 0;
}

void net_set_hook(net_hook_e type, net_hook_f *hook)
{
    ngd.hook[type] = hook;
}

/**
 * Receive an ICMP packet. We deal with REDIRECT and PING here, and silently
 * drop others.
 *
 * @parma ip	IP packet containing the ICMP
 */
static void icmp_receive(struct ip_udp_hdr *ip, int len,
			ipaddr_t src_ip, struct ethernet_hdr *et)
{
    struct icmp_hdr *icmph = (struct icmp_hdr *)&ip->udp_src;

    switch (icmph->type) {

    case ICMP_REDIRECT:
        if (icmph->code != ICMP_REDIR_HOST)
            return;
        //net_debug("ICMP Host Redirect to %pI4", &icmph->un.gateway);
        break;
    default:
        ping_receive(et, ip, len);
        break;
    }
}

/*
 * Network stack initialization
 * 
 * @addr    : ip address
 * @netmask : subnet mask
 * @hwaddr  : hardware address(mac address)
 */
qe_ret net_stack_init(ipaddr_t ip, ipaddr_t netmask, qe_u8 hwaddr[6])
{

    qe_memset(&ngd, 0x0, sizeof(ngd));
    net_copy_ip(&ngd.ipaddr, &ip);
    net_copy_ip(&ngd.netmask, &netmask);
    qe_memcpy(&ngd.hwaddr, hwaddr, 6);
    eth_write_hwaddr(eth_get_dev(), hwaddr);

    net_debug("net stack init");
    net_debug("ipaddr:%p", ip);
    net_debug("netmask:%p", netmask);
    net_debug("hwaddr:"MAC_DUMP_FMT, MAC_DUMP_ARRAY(hwaddr));

    return qe_ok;
}

qe_ret net_udp_send(qe_u8 *buf, ipaddr_t dest, qe_u8 *ether,
    int dport, int sport, int length)
{
    qe_u8 *pkt;
    int eth_hdr_size;
    int pkt_hdr_size;
    
    pkt = buf;

    eth_hdr_size = net_set_ether(pkt, ether, PROT_IP);
    pkt += eth_hdr_size;
    net_set_udp_header(pkt, dest, dport, sport, length);
    pkt_hdr_size = eth_hdr_size + IP_UDP_HDR_SIZE;

    /* if MAC address was not discovered yet, do an ARP request */
    if (qe_memcmp(ether, ngd.ether_null, 6) == 0) {
        net_debug("sending ARP for %p", dest);
        arp_request();
        return qe_ok;
    } else {
        //net_debug("send udp");
        return eth_send(buf, pkt_hdr_size + length);
    }
}

void net_receive(qe_u8 *pkt, int len)
{
    struct ethernet_hdr *ehdr;
    struct ip_udp_hdr *ip;
    ipaddr_t dst_ip;
    ipaddr_t src_ip;
    int et_proto;

    //net_debug("packet receive");

    ehdr = (struct ethernet_hdr *)pkt;

    /* too small packet? */
    if (len < ETHER_HDR_SIZE)
        return;

    et_proto = qe_htons(ehdr->protlen);
    ip = (struct ip_udp_hdr *)(pkt + ETHER_HDR_SIZE);
    len -= ETHER_HDR_SIZE;

    //net_debug("receive from protocol 0x%x", et_proto);

    switch (et_proto) {

    case PROT_ARP:
        arp_receive(ehdr, ip, len);
        break;

    case PROT_IP:
        /* Before we start poking the header, make sure it is there */
        if (len < IP_UDP_HDR_SIZE) {
            //net_debug("len bad %d < %d", len, IP_HDR_SIZE);
            return;
        }
        /* Check the packet length */
        if (len < qe_htons(ip->ip_len)) {
            //net_debug("bad %d < %d", len, qe_htons(ip->ip_len));
            return;
        }
        len = qe_htons(ip->ip_len);
        //net_debug("len=%d v=%02x", len, ip->ip_hl_v & 0xFF);

        /* Can't deal with anything except IPv4 */
        if ((ip->ip_hl_v & 0xF0) != 0x40) {
            //net_debug("not IPv4");
            return;
        }
        /* Can't deal with IP options (headers != 20 bytes) */
		if ((ip->ip_hl_v & 0x0F) > 0x05) {
			return;
        }
        /* Check the Checksum of the header */
		if (!net_cksum_ok((qe_u8 *)ip, IP_HDR_SIZE / 2)) {
			return;
		}
        /* If it is not for us, ignore it */
        dst_ip = net_read_ip(&ip->ip_dst);
        if (ngd.ipaddr && dst_ip != ngd.ipaddr && dst_ip != 0xFFFFFFFF) {
            if (ngd.hook[NET_HOOK_NOT_LOCAL]) 
                ngd.hook[NET_HOOK_NOT_LOCAL](ehdr, ip, qe_htons(ip->udp_len) - UDP_HDR_SIZE);
            return;
        }
        
        if (ip->ip_p == IPPROTO_ICMP) {
            icmp_receive(ip, len, src_ip, ehdr);
        } else if (ip->ip_p != IPPROTO_UDP) {
            return;
        }
        
        /*
        net_debug("received UDP (to=%p, from=%p, len=%d)",
			&dst_ip, &src_ip, len);*/

		/*
		 *	IP header OK.  Pass the packet to the current handler.
		 */
        if (ngd.hook[NET_HOOL_LOCAL_UDP]) {
            ngd.hook[NET_HOOL_LOCAL_UDP](ehdr, ip, qe_htons(ip->udp_len) - UDP_HDR_SIZE);
        }
        break;
    }

    return;
}

int net_update_ether(struct ethernet_hdr *et, qe_u8 *addr, qe_u32 prot)
{
    qe_memcpy(et->dst, addr, 6);
    qe_memcpy(et->src, ngd.hwaddr, 6);
    et->protlen = qe_htons(prot);
    return ETHER_HDR_SIZE;
}

#if defined(CONFIG_USE_NETSYS)
static qe_init int net_sys_init(void)
{
    net_debug("net sys init");
	eth_initialize();
	return 0;
};
QE_COMPONENT_EXPORT(net_sys_init);
#endif
